
#include <SimpleFOC.h>

// BLDC motor & driver instance
BLDCMotor motor = BLDCMotor(11);
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);

// encoder instance
Encoder encoder = Encoder(2, 3, 500);

// channel A and B callbacks
void doA()
{ encoder.handleA(); }

void doB()
{ encoder.handleB(); }

// commander communication instance
Commander command = Commander(Serial);

void doMotor(char *cmd)
{ command.motor(&motor, cmd); }

void setup()
{

    // initialize encoder sensor hardware
    encoder.init();
    encoder.enableInterrupts(doA, doB);
    // link the motor to the sensor
    motor.linkSensor(&encoder);

    // driver config
    // power supply voltage [V]
    driver.voltage_power_supply = 12;
    driver.init();
    // link driver
    motor.linkDriver(&driver);

    // set control loop type to be used
    motor.controller = MotionControlType::torque;

    // contoller configuration based on the controll type
    motor.PID_velocity.P = 0.05;
    motor.PID_velocity.I = 1;
    motor.PID_velocity.D = 0;
    // default voltage_power_supply
    motor.voltage_limit = 12;

    // velocity low pass filtering time constant
    motor.LPF_velocity.Tf = 0.01;

    // angle loop controller
    motor.P_angle.P = 20;
    // angle loop velocity limit
    motor.velocity_limit = 20;

    // use monitoring with serial for motor init
    // monitoring port
    Serial.begin(115200);
    // comment out if not needed
    motor.useMonitoring(Serial);
    motor.monitor_downsample = 0; // disable intially
    motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; // monitor target velocity and angle

    // initialise motor
    motor.init();
    // align encoder and start FOC
    motor.initFOC();

    // set the inital target value
    motor.target = 2;

    // subscribe motor to the commander
    command.add('M', doMotor, "motor");

    // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com)
    Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V."));

    _delay(1000);
}


void loop()
{
    // iterative setting FOC phase voltage
    motor.loopFOC();

    // iterative function setting the outter loop target
    motor.move();

    // motor monitoring
    motor.monitor();

    // user communication
    command.run();
}